#include <clustering_speed_test/occupancy_cluster.h>

void Occupancy_cluster::clustering_(const double _grid_size,const int _BFS_puff_size ,const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) {
  clustering_config.grid_res = _grid_size;
  clustering_config.BFS_puff_size = _BFS_puff_size;
  clustered_indices.clear();
  cal_cloud_max_min_x_y(_cloud);

  init_grid_matrix();
  
  BEV_project_cloud_and_clustering(_cloud);
  
  _clustering_indices = clustered_indices;
}
void Occupancy_cluster::clustering_(const Occupancy_cluster_config _config,const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) {
  LOG(ERROR) << "Occupancy_cluster::clustering not finished yet.";
}
void Occupancy_cluster::cal_cloud_max_min_x_y(PointCloudPtr _cloud) {

  clustering_config.min_x = 999999;
  clustering_config.max_x = -999999;
  clustering_config.min_y = 999999;
  clustering_config.max_y = -999999;
  
  for(auto point : _cloud->points) {
    if(point.x > clustering_config.max_x) {
      clustering_config.max_x = point.x;
    }
    if(point.y > clustering_config.max_y) {
      clustering_config.max_y = point.y;
    }
    if(point.x < clustering_config.min_x) {
      clustering_config.min_x = point.x;
    }
    if(point.y < clustering_config.min_y) {
      clustering_config.min_y = point.y;
    }
  }
  clustering_config.is_config_valid = true;

  int index = 0;
  for(int i = - clustering_config.BFS_puff_size;i <= clustering_config.BFS_puff_size;i++){
    for(int j = -clustering_config.BFS_puff_size;j <= clustering_config.BFS_puff_size;j++){
      if(i == 0 && j == 0 ) continue;
      BFS_puff_seeds.emplace_back(i,j);
    }
  }
}
void Occupancy_cluster::init_grid_matrix() {

  if(!clustering_config.is_config_valid) {
    LOG(ERROR) << "clustering_config is not valid.make share clustering_config is valid then call init_grid_matrix.";
    exit(0);
    return;
  }
  grid_height = static_cast<int>(std::ceil((clustering_config.max_y - clustering_config.min_y)/clustering_config.grid_res));
  grid_width = static_cast<int>(std::ceil((clustering_config.max_x - clustering_config.min_x)/clustering_config.grid_res));
  clustering_matrix = std::vector<std::vector<int>>(grid_height,std::vector<int>(grid_width));
  every_grid_points_indices = std::vector<std::vector<std::vector<int>>>(grid_height, std::vector<std::vector<int>>(grid_width, std::vector<int>()));
}
void Occupancy_cluster::BEV_project_cloud_and_clustering(PointCloudPtr _cloud) {

  for(int point_index = 0;point_index < _cloud->points.size();point_index++) {
    int x_index,y_index;
    if(!cal_clustering_index(_cloud -> points[point_index].x,_cloud -> points[point_index].y,x_index,y_index)) {
      continue;
    }
    clustering_matrix[y_index][x_index]++;
    every_grid_points_indices[y_index][x_index].push_back(point_index);
  }

  BFS_clustering();
}
void Occupancy_cluster::BFS_clustering() {

  clustering_nums = 0;
  for(int y_index = 0;y_index < clustering_matrix.size();y_index++) {
    for(int x_index = 0;x_index < clustering_matrix[0].size();x_index++) {
      if(clustering_matrix[y_index][x_index] <= occupied_th) continue;
      clustering_nums++; 
      std::vector<std::pair<int,int>> one_clustering = BFS_from_here(y_index,x_index); 
      pcl::PointIndices this_cluster_indice;
      for(int clustering_grid_index = 0;clustering_grid_index < one_clustering.size();clustering_grid_index++) {
        std::vector<int> this_grid_points_indices_ = every_grid_points_indices[one_clustering[clustering_grid_index].first][one_clustering[clustering_grid_index].second];
        this_cluster_indice.indices.insert(this_cluster_indice.indices.end(),this_grid_points_indices_.begin(),this_grid_points_indices_.end());
      }
      clustered_indices.push_back(this_cluster_indice);
    }
  }
}
std::vector<std::pair<int,int>> Occupancy_cluster::BFS_from_here(int _y_index,int _x_index) {

  std::vector<std::pair<int,int>> clusterint_res_;
  std::queue<std::pair<int,int>> grid_index_q_;
  grid_index_q_.emplace(_y_index,_x_index);
  clustering_matrix[_y_index][_x_index] = -1 * clustering_nums;

  while(!grid_index_q_.empty()) {
    std::pair<int,int> temp_index = grid_index_q_.front();
    grid_index_q_.pop();
    clusterint_res_.emplace_back(temp_index.first,temp_index.second);

    for(int seed_index = 0;seed_index < BFS_puff_seeds.size();seed_index++) {
      int temp_index_y = temp_index.first + BFS_puff_seeds[seed_index].first;
      int temp_index_x = temp_index.second + BFS_puff_seeds[seed_index].second;
      if(temp_index_y < 0 || temp_index_y >= clustering_matrix.size() || temp_index_x < 0 || temp_index_x >= clustering_matrix[0].size()) {
        continue;
      } 
      if(clustering_matrix[temp_index_y][temp_index_x] > occupied_th) {
        grid_index_q_.emplace(temp_index_y,temp_index_x);
        clustering_matrix[temp_index_y][temp_index_x] = -1 * clustering_nums;
      }
    }
  }
  return clusterint_res_;
}
bool Occupancy_cluster::cal_clustering_index(const double _point_x,const double _point_y,int& _matrix_x,int& _matrix_y) {

  _matrix_x = static_cast<int>(std::floor((_point_x - clustering_config.min_x) / clustering_config.grid_res));
  _matrix_y = static_cast<int>(std::floor((_point_y - clustering_config.min_y) / clustering_config.grid_res));
  if(_matrix_x < 0 || _matrix_x >= grid_width || _matrix_y < 0 || _matrix_y >= grid_height) return false;
  return true; 
}
void Occupancy_cluster::clustering(const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) {
  int puff_size = node["puff_size"].as<int>();
  double grid_size = node["grid_size"].as<double>();
  this->clustering_(grid_size,puff_size,_cloud,_clustering_indices);
}